#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <rosbag/bag.h>
#include <cv_bridge/cv_bridge.h>
#include <boost/filesystem.hpp>
#include <opencv2/opencv.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <string>


typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> policy;

auto pkg_path = boost::filesystem::path(ros::package::getPath("bag_dealer")) / "data";
static size_t index_synced = 1;

void sync_cb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::PointCloud2ConstPtr& pc_msg)
{
    //save synchronized image and point cloud here
    auto img_path = pkg_path / "image";
    auto pc_path = pkg_path / "point_cloud";

    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_msg, image_msg->encoding);
    auto img_cv = cv_ptr->image;
    cv::cvtColor(img_cv, img_cv, CV_RGB2BGR);
    cv::imwrite((img_path / std::to_string(index_synced)).string() + ".jpg", img_cv);

    pcl::PCDWriter pc_writer;
    pcl::PointCloud<pcl::PointXYZI> cloud;
    pcl::fromROSMsg(*pc_msg, cloud);
    pc_writer.write((pc_path / std::to_string(index_synced)).string() + ".pcd", cloud);

    ++index_synced;
}

int main(int argc, char** argv)
{
    if (argc < 3)
    {
        ROS_WARN_STREAM("usage: data_extractor image_topic pc_topic");
        exit(1);
    }

    ros::init(argc, argv, "synced_data_extractor");
    ros::NodeHandle nh_;

    message_filters::Subscriber<sensor_msgs::Image> image_sub(nh_, argv[1], 10);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh_, argv[2], 10);
    message_filters::Synchronizer<policy> sync(policy(10), image_sub, pc_sub);

    sync.registerCallback(boost::bind(&sync_cb, _1, _2));
    ros::spin();

    return 0;
}